#include "pid.h"
#include "math.h"

//将PID函数放在RAM中
#pragma arm section code = "RAMCODE"

void PidCalc(PID *v)
{
    // Compute the error
    v->Err = v->Ref - v->Fdb;
    // Compute the proportional output
    v->Up = v->Kp*v->Err;
    // Compute the integral output
    v->Ui = v->Ui + v->Ki*v->Up + v->Kc*v->SatErr;
    // Compute the derivative output
    v->Ud = v->Kd*(v->Up - v->Up1);
    // Compute the pre-saturated output
    v->OutPreSat = v->Up + v->Ui + v->Ud;

    //对积分项输出限幅
    if (v->Ui > v->UiOutMax)
    {
        v->Ui = v->UiOutMax;
    }
    if (v->Ui < ((-1) * v->UiOutMax))
    {
        v->Ui = (-1) * v->UiOutMax;
    }

    // Saturate the output
    if (v->OutPreSat > v->OutMax)
        v->Out =  v->OutMax;
    else if (v->OutPreSat < ((-1) * v->OutMax))
        v->Out =  (-1) * v->OutMax;
    else
        v->Out = v->OutPreSat;

    // Compute the saturate difference
    //饱和差（当且仅当out值达到饱和之后才会有值）
    v->SatErr = v->Out - v->OutPreSat;
    // Update the previous proportional output
    v->Up1 = v->Up;
}

void PidCalc2(PID *v)
{
    // Compute the error
    v->Err = v->Ref - v->Fdb;

    //算Kp
    v->Kp = v->Ap + v->Bp * (1- exp(-v->Cp * fabs(v->Err)));
    //算Ki
    v->Ki = v->Ai * exp(-v->Ci * fabs(v->Err));
    //算Kd
    v->Kd = v->Ad - v->Bd * (1- exp(-v->Cd * fabs(v->Err)));

    // Compute the proportional output
    v->Up = v->Kp*v->Err;
    // Compute the integral output
    //	v->Ui = v->Ui + v->Ki*v->Up + v->Kc*v->SatErr;

    //积分分离PID控制算法
    if (fabs(v->Err) > v->ErrRef)
    {
        v->Kl = 0.0f;
        v->Ui = 0.0f;
    }
    else
        v->Kl = 1.0f;

    //反计算抗饱和法
    v->Ui = v->Ui + v->Kl*v->Ki*v->Up + v->Kc*v->SatErr;


    if ( (v->Err > 0 && v->Ui > 0) || (v->Err < 0 && v->Ui < 0) )
    {
        //误差限额
        if (v->Ui > 0.2f)
            v->Ui = 0.2f;
        if (v->Ui < -0.2f)
            v->Ui = -0.2f;
    }
    else
        v->Ui = 0;




    // Compute the derivative output
    v->Ud = v->Kd*(v->Up - v->Up1);
    // Compute the pre-saturated output
    v->OutPreSat = v->Up + v->Ui + v->Ud;


    // Saturate the output
    if (v->OutPreSat > v->OutMax)
        v->Out =  v->OutMax;
    else if (v->OutPreSat < ((-1) * v->OutMax))
        v->Out =  (-1) * v->OutMax;
    else
        v->Out = v->OutPreSat;

    // Compute the saturate difference
    v->SatErr = v->Out - v->OutPreSat;
    // Update the previous proportional output
    v->Up1 = v->Up;
}

void PidClear(PID *v)
{
    v->Ui = 0;
    v->Ud = 0;
}

#pragma arm section

